#include <EQFilter.h>
#include <math.h>
#include <assert.h>

EQFilter::EQFilter()
{
	m_isValid = false;
	memset(m_x, 0, sizeof(m_x));
	memset(m_y, 0, sizeof(m_y));
	EQInit();
}

EQFilter::~EQFilter()
{

}

void EQFilter::EQInit()
{
	m_CenterFreq[0] = 31;
	m_CenterFreq[1] = 63;
	m_CenterFreq[2] = 125;
	m_CenterFreq[3] = 250;
	m_CenterFreq[4] = 500;
	m_CenterFreq[5] = 1000;
	m_CenterFreq[6] = 2000;
	m_CenterFreq[7] = 4000;
	m_CenterFreq[8] = 8000;
	m_CenterFreq[9] = 16000;
	memset(m_Gain, 0, sizeof(m_Gain));
	for (size_t i = 0; i < EQ_COUNT; i++)
	{
		m_Q[i] = 2.0;
	}
	for (size_t i = 0; i < EQ_COUNT; i++)
	{
		setEQArgs(i, m_CenterFreq[i], m_Gain[i], m_Q[i], 48000);
	}
}

void EQFilter::setEQArgs(double *center_freq, double *gain, double *Q, double sampling_rate /*= 48000*/)
{
	/*m_CenterFreq = center_freq;
	m_Gain = gain;
	m_Q = Q;
	m_SampleRate = sampling_rate;*/
	memcpy(m_CenterFreq, center_freq, sizeof(double)*EQ_COUNT);
	memcpy(m_Gain, gain, sizeof(double)*EQ_COUNT);
	memcpy(m_Q, Q, sizeof(double)*EQ_COUNT);
	for (size_t i = 0; i < EQ_COUNT; i++)
	{
		setEQArgs(i, m_CenterFreq[i], m_Gain[i], m_Q[i], sampling_rate);
	}
	
}

void EQFilter::setEQArgs(unsigned int count, double center_freq, double gain, double Q, double sampling_rate /*= 48000*/)
{
	double pi = 3.14159265358979323846;
	double w0 = (center_freq / sampling_rate) * pi * 2.0;
	double alpha = sin(w0) / (2.0 * Q);
	double A = pow(10, gain/40.0);
	//Peaking Filter
	//A  = sqrt(pow(10, gain/20)) = pow(10, gain/40)
	//w0 = 2*pi/center_freq/fs
	//alpha = sin(w0)/(2*Q)
	//b0 = 1+alpha*A
	//b1 = -2*cos(w0)
	//b2 = 1-alpha*A
	//a0 = 1+alpha/A
	//a1 = -2*cos(w0)
	//a2 = 1-alpha/A
	if (fabs(gain) < 1e-6)
	{
		m_a[count][0] = 1.0;
		m_a[count][1] = 0;
		m_a[count][2] = 0;
		m_b[count][0] = 1.0;
		m_b[count][1] = 0;
		m_b[count][2] = 0;
	}
	else
	{
		//Low-Shelf Filter
		if (count == 0)
		{
			m_b[count][0] = A * ((A + 1) - (A - 1) * cos(w0) + 2 * sqrt(A) * alpha);
			m_b[count][1] = 2 * A * ((A - 1) - (A + 1) * cos(w0));
			m_b[count][2] = A * ((A + 1) - (A - 1) * cos(w0) - 2 * sqrt(A) * alpha);
			m_a[count][0] = (A + 1) + (A - 1) * cos(w0) + 2 * sqrt(A) * alpha;
			m_a[count][1] = -2 * ((A - 1) + (A + 1) * cos(w0));
			m_a[count][2] = (A + 1) + (A - 1) * cos(w0) - 2 * sqrt(A) * alpha;
		}
		else if(count == 9)//High-Shelf Filter
		{
			m_b[count][0] = A * ((A + 1) + (A - 1) * cos(w0) + 2 * sqrt(A) * alpha);
			m_b[count][1] = -2 * A * ((A - 1) + (A + 1) * cos(w0));
			m_b[count][2] = A * ((A + 1) + (A - 1) * cos(w0) - 2 * sqrt(A) * alpha);
			m_a[count][0] = (A + 1) - (A - 1) * cos(w0) + 2 * sqrt(A) * alpha;
			m_a[count][1] = 2 * ((A - 1) - (A + 1) * cos(w0));
			m_a[count][2] = (A + 1) - (A - 1) * cos(w0) - 2 * sqrt(A) * alpha;
		}
		else
		{
			m_b[count][0] = 1.0 + alpha * A;
			m_b[count][1] = -2.0 * cos(w0);
			m_b[count][2] = 1.0 - alpha * A;
			m_a[count][0] = 1.0 + alpha / A;
			m_a[count][1] = -2.0 * cos(w0);
			m_a[count][2] = 1.0 - alpha / A;
		}
		
		
	}

	m_isValid = true;
}

std::vector<float> EQFilter::run(std::vector<float> &x)
{
	unsigned int length = x.size();
	std::vector<float> y(length, 0);
	float in_x = 0;
	float out_y = 0;
	for (size_t i = 0; i < length; i++)
	{
		in_x = x[i];
		for (size_t j = 0; j < EQ_COUNT; j++)
		{
			//IIR difference equation
			//a[0]*y[i]=b[0]*x[i]+b[1]*x[i-1]+b[2]*x[i-2]-a[1]*y[i-1]-a[2]*y(n-2)
			//y[i]=(b[0]*x[i]+b[1]*x[i-1]+b[2]*x[i-2]-a[1]*y[i-1]-a[2]*y(n-2))/a[0]
			out_y = (in_x * m_b[j][0] + m_x[j][0] * m_b[j][1] + m_x[j][1] * m_b[j][2]
				- m_y[j][0] * m_a[j][1] - m_y[j][1] * m_a[j][2]) / m_a[j][0];
			m_x[j][1] = m_x[j][0];
			m_x[j][0] = in_x;
			m_y[j][1] = m_y[j][0];
			m_y[j][0] = out_y;
			in_x = out_y;
		}
		y[i] = out_y;
	}
	return y;
}

